/*
 * 
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SmartDashboard;

/**
 *
 * @author 
 */
public class Hand {

    // Motors and sensor.
    private Victor m_topFinger;
    private Victor m_bottomFinger;
    private DigitalInput m_palmSensor;
    // Joystick values.
    private Joystick m_joystick;
    private int m_joyCollectButton;
    private int m_joyReleaseButton;
    private int m_joyUpButton;
    private int m_joyDownButton;
    // Waldo values.
    private DriverStationEnhancedIO m_enhancedIO = DriverStation.getInstance().getEnhancedIO();
    private int m_waldoCollectButton;
    private int m_waldoReleaseButton;
    private int m_waldoUpButton;
    private int m_waldoDownButton;
    //Possible motor values.
    private static final double kReleaseSpeed = -.2;
    private static final double kCollectingSpeed = .2;
    private static final double kHoldingSpeed = 0;
    private static final double kFastFingerRotateSpeed = .2;
    private static final double kSlowFingerRotateSpeed = -.2;
    //State maching possible values.
    public static final int kStateReleasing = -1;
    public static final int kStateOff = 0;
    public static final int kStateCollecting = 1;
    // Keep holding state private, since it is based on the palm sensor
    private static final int kStateHolding = 2;
    public static final int kStateRotateUp = 3;
    public static final int kStateRotateDown = 4;
    private int m_state = kStateOff;
    private int m_requestedAutonomousState = kStateOff;
    private DriverStation m_ds = DriverStation.getInstance();

    /**
     *
     * @param digitalSlot
     * @param topFingerChannel
     * @param bottomFingerChannel
     * @param palmChannel
     */
    public Hand(int digitalSlot, int topFingerChannel, int bottomFingerChannel, int palmChannel, int waldoCollectButton, int waldoReleaseButton, int waldoUpButton, int waldoDownButton) {
        m_topFinger = new Victor(digitalSlot, topFingerChannel);
        m_bottomFinger = new Victor(digitalSlot, bottomFingerChannel);
        m_palmSensor = new DigitalInput(digitalSlot, palmChannel);
        m_waldoCollectButton = waldoCollectButton;
        m_waldoReleaseButton = waldoReleaseButton;
        m_waldoUpButton = waldoUpButton;
        m_waldoDownButton = waldoDownButton;
    }

    /**
     * Sets the joystick controlling the hand. This not part of the constructor to allow the information to
     * change.
     * @param joy The joystick controlling the hand.
     * @param collectButton Press to start collecting tubes.
     * @param releaseButton Press to release tube. Press and hold to push tube out.
     * @param upButton Hold to rotate the tube up.
     * @param downButton Hold to rotate tube down.
     */
    public void setJoystick(Joystick joy, int collectButton, int releaseButton, int upButton, int downButton) {
        m_joystick = joy;
        m_joyCollectButton = collectButton;
        m_joyReleaseButton = releaseButton;
        m_joyUpButton = upButton;
        m_joyDownButton = downButton;
    }

    /**
     * Called when you want the state of the robot hand to be updated.  Changes the speeds on the finger
     * motors based on the state of the buttons and sensors. If in autonomous, it will update the state
     * based on the requested state.
     */
    public void update() {
        int previousState = m_state;
        if (m_ds.isAutonomous()) {
            getAutonomousState();
        } else {
            getState();
        }

        //Make sure the state has changed before setting the motors.
        if (m_state != previousState) {
            switch (m_state) {
                case kStateReleasing:
                    m_topFinger.set(kReleaseSpeed);
                    m_bottomFinger.set(kReleaseSpeed);
                    break;
                case kStateCollecting:
                    m_topFinger.set(kCollectingSpeed);
                    m_bottomFinger.set(kCollectingSpeed);
                    break;
                case kStateHolding:
                    m_topFinger.set(kHoldingSpeed);
                    m_bottomFinger.set(kHoldingSpeed);
                    break;
                case kStateRotateUp:
                    m_topFinger.set(kFastFingerRotateSpeed);
                    m_bottomFinger.set(kSlowFingerRotateSpeed);
                    break;
                case kStateRotateDown:
                    m_topFinger.set(kSlowFingerRotateSpeed);
                    m_bottomFinger.set(kFastFingerRotateSpeed);
                    break;
                case kStateOff:
                default:
                    m_topFinger.set(0);
                    m_bottomFinger.set(0);
                    break;
            }
        }
    }

    /**
     * Looks at the joystick buttons, palm sensor, and previous state to determine the state
     * of the robot hand. 
     * @return The hand state based on the values of the sensor and buttons.
     */
    private void getState() {

        try {
            // Get the ON/OFF/releasing state.
            if (m_joystick.getRawButton(m_joyCollectButton) || m_enhancedIO.getDigital(m_waldoCollectButton)) {
                m_state = kStateCollecting;
            } else if (m_joystick.getRawButton(m_joyReleaseButton) || m_enhancedIO.getDigital(m_waldoReleaseButton)) {
                m_state = kStateReleasing;
            } else if (m_state == kStateReleasing) {
                m_state = kStateOff;
            }

            // If ON check whether to collect, hold, rotate up or rotate down.
            if (m_state > 0) {
                // Always collect when the palm sensor isn't activated.
                if (!m_palmSensor.get()) {
                    if (m_joystick.getRawButton(m_joyUpButton) || m_enhancedIO.getDigital(m_waldoUpButton)) {
                        m_state = kStateRotateUp;
                    } else if (m_joystick.getRawButton(m_joyDownButton) || m_enhancedIO.getDigital(m_waldoDownButton)) {
                        m_state = kStateRotateDown;
                    } else {
                        m_state = kStateHolding;
                    }
                } else {
                    m_state = kStateCollecting;
                }
            }
            SmartDashboard.log(m_state, "State");
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Hand Error!");
            m_state = kStateOff;
        }
    }

    /**
     * Gets the state for the hand based on the requested autonomous state
     * and the value of the palm sensor.
     */
    private void getAutonomousState() {
        // Get the ON/OFF/releasing state.
        if (m_requestedAutonomousState == kStateCollecting) {
            m_state = kStateCollecting;
        } else if (m_requestedAutonomousState == kStateReleasing) {
            m_state = kStateReleasing;
        } else if (m_requestedAutonomousState == kStateOff) {
            m_state = kStateOff;
        }

        // If ON check whether to collect, hold, rotate up or rotate down.
        if (m_state > 0) {
            // Always collect when the palm sensor isn't activated.
            if (!m_palmSensor.get()) {
                if (m_requestedAutonomousState == kStateRotateUp) {
                    m_state = kStateRotateUp;
                } else if (m_requestedAutonomousState == kStateRotateDown) {
                    m_state = kStateRotateDown;
                } else {
                    m_state = kStateHolding;
                }
            } else {
                m_state = kStateCollecting;
            }
        }

    }

    /**
     * The state requested for the hand.  The update function must be called for
     * the state to change.  Depending on the palm sensor, the requested state
     * may or may not happen.
     * @param state The requested state of the hand.
     */
    public void requestAutonomousState(int state) {
        m_requestedAutonomousState = state;
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {
        SmartDashboard.log(m_state, "Hand");
    }
}
